/*
 * Acelerometer.c
 *
 *  Created on: Jun 8, 2010
 *      Author: iandrono
 */

#include "acelerometer.h"
#include "../adc/adc.h"
#include "../defines.h"
#include <util/delay.h>
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
#include "../uart/uart.h"
#include "../leds/leds.h"
#include "../timer/timer.h"

#define ACELEROMETER_VSS 3.3
#define ACELEROMETER_SENSITIVITY 300
#define ACELCOUNTER_MEMORY 10
#define DEBUGZERO 1.138824

typedef struct {
	volatile double xZero;
	volatile adcInputType xInput;
	volatile double yZero;
	volatile adcInputType yInput;
	volatile double zZero;
	volatile adcInputType zInput;
} acelerometerType;

volatile acelerometerType config;

volatile double lastAcelMeasure;
volatile long acelCounter[ACELCOUNTER_MEMORY];
volatile int acelCounterIndex;

//Variables de video/leds
extern volatile int videoMemoryIndex;
extern volatile imageWayType videoMemoryStep;

void initAcelerometer( void ) {
	int j;
	/*
	config.xInput = ADC1;
	config.yInput = ADC2;
	config.zInput = ADC3;

	char backup[200] = {0};

	//Tengo que hacer una primera lectura para setear los puntos iniciales (los ceros)
	_delay_ms(1000);

	changeInputADC( config.xInput );
	unsigned char data = readADC();
	double dataInVolts = ((double)data)*ACELEROMETER_VSS/255;
	config.xZero = dataInVolts;
	sprintf(backup, "initAcelerometer xZero: %f", config.xZero);
	writeStrUart(backup);
	writeUart(10);
	writeUart(13);


	changeInputADC( config.yInput );
	data = ((double)readADC())*ACELEROMETER_VSS/255;
	config.yZero = data;

	changeInputADC( config.zInput );
	data = ((double)readADC())*ACELEROMETER_VSS/255;
	config.zZero = data;
	*/

	holdedByAcelerometer = FALSE;
	acelCounterIndex = 0;
	lastAcelMeasure = DEBUGZERO;

	for( j=0 ; j<ACELCOUNTER_MEMORY ; j++ )
		acelCounter[j] = 0;
}

extern unsigned char delayms;

unsigned char acelerometerAdjust( void ) {
	//changeInputADC( config.xInput );
	unsigned char data = readADC();
	double dataInVolts = ((double)data)*ACELEROMETER_VSS/255;
	int i;
	double newTimerCounter = 0.0;
	double back = 0.0;
	double comp = 0.0;

	char backup[200] = {0};

	//Si la ultima medicion es distinta en signo que la current, hubo cambio de sentido
	if( ((lastAcelMeasure-DEBUGZERO)*(dataInVolts-DEBUGZERO)) < 0 ) {

		if( (lastAcelMeasure-DEBUGZERO) > 0 )
			comp = (lastAcelMeasure-DEBUGZERO)-(dataInVolts-DEBUGZERO);
		else
			comp = (dataInVolts-DEBUGZERO)-(lastAcelMeasure-DEBUGZERO);

		//sprintf(backup, "comp:\t%f", comp);
		//writeStrUart(backup);
		//writeUart(10);
		//writeUart(13);

		if( comp > 0.05 ) {
			newTimerCounter = 0;

			for( i=1 ; i<=(acelCounterIndex+1) ; i++ )
				newTimerCounter += acelCounter[i-1];

			newTimerCounter /= i;
			back = newTimerCounter;

			//newTimerCounter = (getTimerCounter()*newTimerCounter)/ROWS_Q;

			//si el promedio de filas por periodo es mayor que ROWS_Q
			//me esta sobrando tiempo blanco, tengo que ir mas lento, sumo 1
			if( back > ROWS_Q )
				newTimerCounter = getTimerCounter()+5;
			//si el promedio de filas por periodo es menor que ROWS_Q
			//no me alcanza el tiempo, tengo que ir mas rapido, resto 1
			else if( back < ROWS_Q )
				newTimerCounter = getTimerCounter()-5;

			if( ceil(newTimerCounter) < 40 )
				newTimerCounter = 40;
			else if( ceil(newTimerCounter) > 150 )
				newTimerCounter = 150;

			setTimerCounter(ceil(newTimerCounter));

			if( acelCounterIndex+1 != ACELCOUNTER_MEMORY ) {
				acelCounterIndex++;
			} else {
				for( i=0 ; i<ACELCOUNTER_MEMORY-1 ; i++ )
					acelCounter[i] = acelCounter[i+1];

				acelCounter[i] = 0;
			}

			//sprintf(backup, "prom:\t%f\tnewTimerCounter:\t%d",back, getTimerCounter());
			//writeStrUart(backup);
			//writeUart(10);
			//writeUart(13);
		}
	}

	lastAcelMeasure = dataInVolts;
	return newTimerCounter;

	/*
	int i;
	unsigned char newTimerCounter = -1;

	char backup[200] = {0};
	sprintf(backup, "dataInVolts:\t%f\tcurrentMeasure:\t%f\tfila:\t%d\ttimercounter:\t%d\t", dataInVolts,(dataInVolts-DEBUGZERO), videoMemoryIndex, getTimerCounter());
	writeStrUart(backup);
	writeUart(10);
	writeUart(13);

	//Si la ultima medicion es distinta en signo que la current, hubo cambio de sentido
	if( ((lastAcelMeasure-DEBUGZERO)*(dataInVolts-DEBUGZERO)) < 0 && abs((lastAcelMeasure-DEBUGZERO)-(dataInVolts-DEBUGZERO)) > 0.05 ) {
		newTimerCounter = 0;

		for( i=1 ; i<=(acelCounterIndex+1) ; i++ )
			newTimerCounter += acelCounter[i-1];

		newTimerCounter /= i;
		newTimerCounter = abs(ceil((TIMERFACTOR*newTimerCounter)/(ROWS_Q*(TIMERFACTOR/getTimerCounter()))));
		//newTimerCounter = abs(ceil((TIMERFACTOR*newTimerCounter)/(ROWS_Q*(1/(delayms/1000)))));

		if( holdedByAcelerometer ) {
			//Si el acelerometro holdeo, quiere decir que me sobro tiempo, tengo que desacelerar la frecuencia
			holdedByAcelerometer = FALSE;
			setTimerCounter(newTimerCounter);
		}

		if( videoMemoryStep == UP ) {

			if( videoMemoryIndex < ROWS_Q-1 ) {
				//Quede corto, no llegue a barrer toda la imagen, tengo que acelerar la frecuencia
				setTimerCounter(newTimerCounter);
			}

			videoMemoryStep = DOWN;
			videoMemoryIndex--;
		} else if ( videoMemoryStep == DOWN ) {

			if( videoMemoryIndex > 0 ) {
				//Quede corto, no llegue a barrer toda la imagen, tengo que acelerar la frecuencia
				setTimerCounter(newTimerCounter);
			}

			videoMemoryStep = UP;
			videoMemoryIndex++;
		}

		if( acelCounterIndex+1 != ACELCOUNTER_MEMORY ) {
			acelCounterIndex++;
		} else {
			for( i=0 ; i<ACELCOUNTER_MEMORY-1 ; i++ )
				acelCounter[i] = acelCounter[i+1];

			acelCounter[i] = 0;
		}

	} else if( videoMemoryIndex == ROWS_Q || videoMemoryIndex < 0 ) {
		//La aceleracion NO cambio de signo, pero ya barri la imagen, tengo que holdear
		holdedByAcelerometer = TRUE;
		acelCounter[acelCounterIndex]++;
	} else {
		acelCounter[acelCounterIndex]++;
	}
	lastAcelMeasure = dataInVolts;

	return newTimerCounter;
	*/
}

void adjustCounters( void ) {
	acelCounter[acelCounterIndex]++;
}

void adjustImage( imageType * image ) {
	//Aca le hago ajustes a la imagen segun las aceleraciones en y, z
	int i,j,k;

	//Calculo loco
	for( i=0 ; i<COLORS_Q_MAX ; i++ ) {
		for( j=0 ; j<ROWS_Q ; j++ ) {
			for( k=0 ; k<LEDS_Q_MAX/BYTE_SIZE ; k++ )
				(*image)[i][j][k] = (*image)[i][j][k];
		}
	}

}
